#include "DevicePropeller.h"
#include "common.h"

using namespace std;

DevicePropeller::DevicePropeller(const string& interfaceName, const string& dt) {
    deviceName = interfaceName;
    deviceType = dt;

    m_fd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
    if (m_fd < 0) {
        throw system_error(errno, generic_category(), "Failed to create CAN socket");
    }

    ifreq ifr;

    strcpy(ifr.ifr_name, deviceName.c_str());

    // 获取网络接口的索引值
    if (ioctl(m_fd, SIOCGIFINDEX, &ifr) < 0) {
        int err = errno;
        close(m_fd);
        throw system_error(err, generic_category(), "Failed to get interface index");
    }

     // 初始化CAN地址结构体
    sockaddr_can addr;
    addr.can_family = AF_CAN; // 设置地址族为CAN
    addr.can_ifindex = ifr.ifr_ifindex; // 设置接口索引

    // 绑定socket到指定的CAN接口
    if (bind(m_fd, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0) {
        int err = errno;
        close(m_fd);
        throw system_error(err, generic_category(), "Failed to bind CAN socket");
    }
}

DevicePropeller::~DevicePropeller() {
    close(m_fd);
    m_fd = -1;
}

void DevicePropeller::func(Propeller& data) {
    try {
        can_frame frame{};
        frame.can_id = 0x55;
        frame.can_dlc = 8;

        if(data.mode == 1){
            if(data.angle > Propeller_UD_Max || data.angle < Propeller_UD_Min){
                throw out_of_range("角度过大");
            }
        }
        
        if(data.mode == 2){
            if(data.angle > Propeller_LR_Max || data.angle < Propeller_LR_Min){
                throw out_of_range("角度过大");
            }
        }

        create(frame.data, data.mode, data.angle);
        int ret = write(m_fd,&frame,sizeof(frame));
        
        if (ret != sizeof(frame)) {
            printf("send frame error");
            // close(s);
            throw 1;
        }
        printf("发送报文： ");
        // 打印发送的报文内容
        print_resp(frame.data, frame.can_dlc);

        // 接收响应报文
        ret = read(m_fd, &frame, sizeof(frame));
        if (ret < 0) {
            printf("read error");
            // close(s);
            throw 2;
        } else if (ret == 0) {
            printf("No data received\n");
            // close(s);
            throw 3;
        } else {
            printf("响应报文： ");
            // 打印接收到的报文内容
            print_resp(frame.data, frame.can_dlc);
        }

    } catch (const exception& e) {
        cerr << "Error in DevicePropeller::func: " << e.what() << endl;
    }
}

int DevicePropeller::create(unsigned char data[], int mode, int angle) {
    switch (mode) {
        case 1:
            data[2] = 0x32;
            break;
        case 2:
            data[2] = 0x33;
            break;
        case 3:
            data[2] = 0x31;
            break;
        case 4:
            data[0] = 0x23;
            data[1] = 0x00;
            data[2] = 0x11;
            data[3] = 0x00;
            data[4] = 0x00;
            data[5] = 0x00;
            data[6] = 0x00;
            data[7] = 0x34;
            return 0;
        case 5:
            data[0] = 0x23;
            data[1] = 0x00;
            data[2] = 0x12;
            data[3] = static_cast<unsigned char>(angle);
            data[4] = static_cast<unsigned char>(angle >> 8);
            data[5] = static_cast<unsigned char>(angle >> 16);
            data[6] = static_cast<unsigned char>(angle >> 24);
            data[7] = 0x34;
            return 0;
        default:
            return 0;
    }

    data[0] = 0x23;
    data[1] = 0x04;
    data[3] = static_cast<unsigned char>(angle);
    data[4] = static_cast<unsigned char>(angle >> 8);
    data[5] = static_cast<unsigned char>(angle >> 16);
    data[6] = static_cast<unsigned char>(angle >> 24);

    data[7] = 0;
    for (int i = 0; i < 7; ++i) {
        data[7] += data[i];
    }

    return 0;
}